/**
  ******************************************************************************
  * File Name          : .c
  * Description        : 
                     ##### How to use this driver #####
  ==============================================================================
	>>
  ******************************************************************************
  */
#include "matlab_motion.h"
#include "math.h"

/*
 * Arguments    : float u0
 *                float u1
 * Return Type  : float
 */
static float matlab_rt_atan2f_snf(float u0, float u1)
{
  float y;
	if (u1 == 0.0F) {
    if (u0 > 0.0F) {
      y = RT_PIF / 2.0F;
    } else if (u0 < 0.0F) {
      y = -(RT_PIF / 2.0F);
    } else {
      y = 0.0F;
    }
  } else {
    y = (float)atan2(u0, u1);
  }

  return y;
}


/*
 * Arguments    : float xA
 *                float yA
 *                float res[4]
 * Return Type  : void
 */
void matlab_motion_solve(float xA, float yA, float res[4])
{
  int i0;
  float c;
  float N;
  float K;
  float DT3;
  float st3a[2];
  float st4a[2];
  float st1a[4];
  int i1;
  long exitg1;
  float st1aa[2];
  float b_c;
  float Q;
  float S;
  float DT1;
	
	xA *= 10;
	yA *= 10;
  for (i0 = 0; i0 < 4; i0++) {
    res[i0] = -100.0F;
  }

  c = (xA * xA + 28224.0F) + yA * yA;
  N = 336.0F * xA;
  K = 336.0F * yA;
  DT3 = K * K - ((c - 4900.0F) * (c - 4900.0F) - N * N);
  if (DT3 < 0.0F) {
		for (i0 = 0; i0 < 4; i0++) {
			res[i0] = -100.0F;
		}
		return;
  } else {
    st3a[0] = 2.0F * matlab_rt_atan2f_snf(-K + (float)sqrt(DT3), (c - 4900.0F) - N);
    st3a[1] = 2.0F * matlab_rt_atan2f_snf(-K - (float)sqrt(DT3), (c - 4900.0F) - N);

    st4a[0] = matlab_rt_atan2f_snf(yA + 168.0F * (float)sin(st3a[0]), xA + 168.0F *
      (float)cos(st3a[0]));
    st4a[1] = matlab_rt_atan2f_snf(yA + 168.0F * (float)sin(st3a[1]), xA + 168.0F *
      (float)cos(st3a[1]));

    for (i0 = 0; i0 < 4; i0++) {
      st1a[i0] = 0.0F;
    }

    i1 = 0;
    do {
      exitg1 = 0L;
      if (i1 < 2) {
        c = xA + 28.0F * (float)cos(st3a[i1]);
        b_c = yA + 28.0F * (float)sin(st3a[i1]);
        c = c * c + b_c * b_c;
        Q = 140.0F * (xA + 28.0F * (float)cos(st3a[i1]));
        S = 140.0F * (yA + 28.0F * (float)sin(st3a[i1]));
        DT1 = S * S - (((c + 4900.0F) - 19600.0F) * ((c + 4900.0F) - 19600.0F) -
                       Q * Q);
        if (DT1 < 0.0F) {
					for (i0 = 0; i0 < 4; i0++) {
						res[i0] = -100.0F;
					}
					return;
        } else {
          st1a[i1] = 2.0F * matlab_rt_atan2f_snf(S + (float)sqrt(DT1), ((c + 4900.0F) -
            19600.0F) + Q);
          st1a[2 + i1] = 2.0F * matlab_rt_atan2f_snf(S - (float)sqrt(DT1), ((c +
            4900.0F) - 19600.0F) + Q);
          i1++;
        }
      } else {
        st1aa[0] = st1a[0];
        st1aa[1] = st1a[3];
        for (i0 = 0; i0 < 2; i0++) {
          res[i0] = st1aa[i0];
          res[i0 + 2] = st4a[i0];
        }

        exitg1 = 1L;
      }
    } while (exitg1 == 0L);
  }
}
